#include "rtk_pub/rtk_pub.h"

namespace NightFall
{
    RTK::RTK() : Node("rtk_pub_node")
    {
        rtk_pub_ = this->create_publisher<rtk_interface::msg::RTKMSG>("/rtk", 10 /* rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()*/);

        getParam();
    }

    RTK::~RTK()
    {
        ser_.close();
        if (!ser_.isOpen())
        {
            RCLCPP_INFO_STREAM(this->get_logger(), "Serial closed.");
        }
    }

    void RTK::getParam()
    {
        baudrate_ = this->declare_parameter<int>("baudrate", 0);
        port_ = this->declare_parameter<std::string>("port", "");

        RCLCPP_INFO(this->get_logger(), "baudrate: %d", baudrate_);
        RCLCPP_INFO(this->get_logger(), "port: %s", port_.c_str());
    }

    int RTK::serialIntialize()
    {
        try
        {
            ser_.setPort(port_);
            ser_.setBaudrate(baudrate_);
            serial::Timeout To = serial::Timeout::simpleTimeout(100);
            ser_.setTimeout(To);
            ser_.open();
        }
        catch (const serial::SerialException &se)
        {
            RCLCPP_ERROR_STREAM(this->get_logger(), "Serial open failed.");
            RCLCPP_ERROR_STREAM(this->get_logger(), se.what());
            return 1;
        }
        return 0;
    }

    void RTK::pubRtkMsg()
    {
        while (rclcpp::ok())
        {
            int year_, month_, day_, hour_, minute_;
            double seconds_;
            double latitude_, longitude_, northing_, easting_, undulation_, velocity_, azimuth_angle_, magnetic_declination_;
            std::string magnetic_direction_;
            int fix_state_;
            std::string zone_;
            std::string buf_str_;
            std::vector<std::string> parm_str_vec_;
            if (size_t num = ser_.available(); num > 100)
            {
                ser_.readline(buf_str_, ser_.available());
                std::cout << buf_str_ << std::endl;
                boost::split(parm_str_vec_, buf_str_, boost::is_any_of(","), boost::token_compress_on);
                bool bl = buf_str_[0] == '$';
                if (bl)
                {
                    if (buf_str_.find("$GNRMC"))
                    {
                        // hour_ = stoi(parm_str_vec_.at(1).substr(0, 2));
                        // minute_ = stoi(parm_str_vec_.at(1).substr(2, 2));
                        // seconds_ = stod(parm_str_vec_.at(1)) - hour_ * 10000.0 - minute_ * 100.0;
                        // latitude_ = stod(parm_str_vec_.at(3));
                        // longitude_ = stod(parm_str_vec_.at(5));
                        // fix_state_ = stoi(parm_str_vec_.at(2));
                        // velocity_ = stod(parm_str_vec_.at(7));
                        // azimuth_angle_ = stod(parm_str_vec_.at(8));
                        // magnetic_declination_ = stod(parm_str_vec_.at(10));
                        // magnetic_direction_ = parm_str_vec_.at(11);
                        // year_ = stoi(parm_str_vec_.at(9).substr(0, 2));
                        // month_ = stoi(parm_str_vec_.at(9).substr(2, 2));
                        // day_ = stoi(parm_str_vec_.at(9).substr(4, 2));
                        // gps_conv::LLtoUTM(latitude_, longitude_, northing_, easting_, zone_);
                        // // rtk_msg_.header.stamp = ;
                        // rtk_msg_.fix_state = fix_state_;
                        // rtk_msg_.northing = northing_;
                        // rtk_msg_.easting = easting_;
                        // rtk_msg_.velocity = velocity_;
                        // rtk_msg_.azimuth_angle = azimuth_angle_;
                        // rtk_msg_.magnetic_declination = magnetic_declination_;
                        // rtk_msg_.magnetic_direction = magnetic_direction_;
                        // rtk_pub_->publish(rtk_msg_);
                        std::cout << buf_str_ << std::endl;
                    }
                }
            }
        }
    }
} // namespace NightFall

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node_ = std::make_shared<NightFall::RTK>();
    if (!node_->serialIntialize())
    {
        node_->pubRtkMsg();
    }
    return 0;
}
